{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Wy0JIcGioHI9"
      },
      "source": [
        "# Rot3"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "YqaxPKyloJG_"
      },
      "source": [
        "A `gtsam.Rot3` represents an orientation or attitude in 3D space. It can be manipulated and presented as a rotation matrix $ R \\in \\mathbb{R}^{3 \\times 3} $, a unit quaternion, roll-pitch-yaw (Euler) angles $ (\\phi, \\theta, \\psi) $, or as an axis-angle representation $ (\\hat{\\omega}, \\theta) $ with $ \\hat{\\omega} \\in \\mathbb{R}^3 $ and $ \\theta \\in \\mathbb{R} $. It models a 3D orientation as both a manifold in $ \\mathcal{SO}(3) $ and as a Lie group in $ \\text{SO}(3) $. Internally, it is stored as a $ 3 \\times 3 $ rotation matrix but can be configured to use quaternions at build time for efficiency."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Hmwbhz75pcQT"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Rot3.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "HyXPOMakoDkY"
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "_fWy46Mepoxh"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import Rot3, Point3, Quaternion\n",
        "import numpy as np"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "3DkkBKyAqGnY"
      },
      "source": [
        "## Initialization"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "RrJ5ZEdhqJPU"
      },
      "source": [
        "A `Rot3` can be initialized in many different ways, which are detailed in this section. Note that printing a `Rot3` displays its 3x3 rotation matrix representation, which in general is a 3x3 matrix where the columns are unit vectors that define the orientation's coordinate frame."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "AqEy5JLe5X1t"
      },
      "source": [
        "### Constructor\n",
        "\n",
        "The `Rot3` constructor provides for initialization with no arguments, yielding the identity rotation (equivalent to $I_3$), initialization with a precalculated rotation matrix (either as a 3x3 `np.ndarray`, as three 3-vectors, or as 9 floats), and initialization with a quaternion's $w, x, y, z$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "mj8X-wIdq6GR",
        "outputId": "48a6921d-df39-4fd8-aaf8-76d4bcdb70b1"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 6.12323e-17, -1;\n",
            "\t0, 1, 6.12323e-17\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t11, 12, 13;\n",
            "\t21, 22, 23;\n",
            "\t31, 32, 33\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t1, 2, 3;\n",
            "\t4, 5, 6;\n",
            "\t7, 8, 9\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t-1, 0, 0;\n",
            "\t0, -1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# No-argument constructor\n",
        "a = Rot3()\n",
        "print(a)\n",
        "\n",
        "# Construct from a rotation matrix\n",
        "theta = np.pi / 2\n",
        "b = Rot3(np.array([ # Rotate around X axis by PI / 2\n",
        "    [1, 0, 0],\n",
        "    [0, np.cos(theta), -np.sin(theta)],\n",
        "    [0, np.sin(theta), np.cos(theta)]\n",
        "]))\n",
        "print(b)\n",
        "\n",
        "# Construct from three column vectors\n",
        "c = Rot3([11, 21, 31], [12, 22, 32], [13, 23, 33])\n",
        "print(c)\n",
        "\n",
        "# Construct from 9 floats\n",
        "d = Rot3(1, 2, 3, 4, 5, 6, 7, 8, 9)\n",
        "print(d)\n",
        "\n",
        "# Construct from quaternion values\n",
        "e = Rot3(0, 0, 0, 1) # Rotate around Z axis by pi\n",
        "print(e)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "EMaB3yVoJ_qv"
      },
      "source": [
        "### Named constructors\n",
        "\n",
        "In addition to its constructors, `Rot3` has several named constructors, or factory functions, that allow instantiation from a wide variety of methods."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "3s9Ym_6BaE_r"
      },
      "source": [
        "`Rot3.Identity()` returns the 3x3 rotation identity matrix."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "GcAB8GtVaLjK",
        "outputId": "b9e701cd-6a3f-4171-a518-158a2f7b60fd"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "print(Rot3.Identity())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "F2MXz29VXqLR"
      },
      "source": [
        "`Rx`, `Ry`, `Rz`, and `RzRyRx` create rotations around these axes."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "w-qh5dX6VWAW",
        "outputId": "8fe29ae6-eb47-4460-c27b-3acd8ee5e5bf"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 6.12323e-17, -1;\n",
            "\t0, 1, 6.12323e-17\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t0.707107, 0, 0.707107;\n",
            "\t0, 1, 0;\n",
            "\t-0.707107, 0, 0.707107\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t0.866025, -0.5, 0;\n",
            "\t0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t0.612372, 0.612372, 0.5;\n",
            "\t0.353553, 0.353553, -0.866025;\n",
            "\t-0.707107, 0.707107, 4.32978e-17\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t0.612372, 0.612372, 0.5;\n",
            "\t0.353553, 0.353553, -0.866025;\n",
            "\t-0.707107, 0.707107, 4.32978e-17\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Rotation around X axis\n",
        "x = Rot3.Rx(np.pi / 2)\n",
        "print(x)\n",
        "\n",
        "# Rotation around Y axis\n",
        "y = Rot3.Ry(np.pi / 4)\n",
        "print(y)\n",
        "\n",
        "# Rotation around Z axis\n",
        "z = Rot3.Rz(np.pi / 6)\n",
        "print(z)\n",
        "\n",
        "# Rotate around Z, then Y, then X axes\n",
        "# Note that the order of angles in the function signature (x, y, z) is the\n",
        "# reverse of the order in which the rotations are applied (z, y, x).\n",
        "zyx = Rot3.RzRyRx(np.pi / 2, np.pi / 4, np.pi / 6)\n",
        "# Rot3.RzRyRx is overloaded: it also accepts an array of [x, y, z].\n",
        "# e.g. the above is identical to zyx = Rot3.RzRyRx([np.pi / 2, np.pi / 4, np.pi / 6]).\n",
        "print(zyx)\n",
        "# Of course, zyx is the same as z * y * x, since we fed the same angles to each.\n",
        "print(z * y * x)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "c-_H7XmUYAd_"
      },
      "source": [
        "Similarly, `Yaw`, `Pitch`, `Roll`, and `Ypr` are available."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "bGEMGXkpYT9t",
        "outputId": "31655b9f-045f-4b51-dff2-42de4382427f"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t0.866025, -0.5, 0;\n",
            "\t0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t0.707107, 0, 0.707107;\n",
            "\t0, 1, 0;\n",
            "\t-0.707107, 0, 0.707107\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 6.12323e-17, -1;\n",
            "\t0, 1, 6.12323e-17\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t0.612372, 0.612372, 0.5;\n",
            "\t0.353553, 0.353553, -0.866025;\n",
            "\t-0.707107, 0.707107, 4.32978e-17\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Yaw around Z axis (positive yaw is to the right, as in aircraft heading)\n",
        "y = Rot3.Yaw(np.pi / 6)\n",
        "print(y)\n",
        "\n",
        "# Pitch around Y axis (positive pitch is up, as in increasing aircraft altitude)\n",
        "p = Rot3.Pitch(np.pi / 4)\n",
        "print(p)\n",
        "\n",
        "# Roll around X axis\n",
        "r = Rot3.Roll(np.pi / 2)\n",
        "print(r)\n",
        "\n",
        "# Yaw, pitch, then roll\n",
        "# Unlike RzRyRx, rotations are applied in the same order as supplied.\n",
        "# Ypr is not overloaded to support an array.\n",
        "ypr = Rot3.Ypr(np.pi / 6, np.pi / 4, np.pi / 2)\n",
        "print(ypr)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "_ks-ohhZZ5Ap"
      },
      "source": [
        "`Rot3.Quaternion` is identical to the four-argument `Rot3` constructor."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "uO9hb2RBaG3g",
        "outputId": "5409ef2e-3651-439b-af9f-6ed7888aa9d5"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t-1, 0, 0;\n",
            "\t0, -1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "True\n"
          ]
        }
      ],
      "source": [
        "# Create from quaternion w, x, y, z\n",
        "q = Rot3.Quaternion(0, 0, 0, 1)\n",
        "print(q)\n",
        "print(q.equals(Rot3(0, 0, 0, 1), 1e-8))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "jy7dn6_vabsK"
      },
      "source": [
        "`Rot3.AxisAngle` creates a `Rot3` from an axis and an angle around that axis."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "M_OOSKgAaqhF",
        "outputId": "cbb875b7-9204-4a90-c225-dbea46718c6f"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t2.22045e-16, 0, 1;\n",
            "\t0, 1, 0;\n",
            "\t-1, 0, 2.22045e-16\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "aa = Rot3.AxisAngle([0, 1, 0], np.pi / 2)\n",
        "print(aa)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ruyunRlUclFX"
      },
      "source": [
        "`Rot3.Rodrigues` creates a `Rot3` from incremental roll, pitch, and yaw values. It is identical to the exponential map at identity."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "NUgeRQzIcqYp",
        "outputId": "8c189748-267b-4c25-e0cf-4eed8721bc4a"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t-0.156058, -0.673795, 0.72225;\n",
            "\t0.982078, -0.0276074, 0.186445;\n",
            "\t-0.105686, 0.738402, 0.666028\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "rod = Rot3.Rodrigues(np.pi / 6, np.pi / 4, np.pi / 2)\n",
        "# Rodrigues is overloaded to support an array.\n",
        "# e.g. Rot3.Rodrigues([np.pi / 6, np.pi / 4, np.pi / 2])\n",
        "print(rod)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "INihgtF6fI22"
      },
      "source": [
        "`Rot3.ClosestTo` finds the closest valid `Rot3` to the input matrix which minimizes the Frobenius norm. The Frobenius norm is a measure of matrix difference:\n",
        "\n",
        "$$\n",
        "||A - B||_F = \\sqrt{\\sum_{i,j} (A_{ij} - B_{ij})^2}\n",
        "$$"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "EFMbwiTKfLfJ",
        "outputId": "72bf7784-6a8c-4052-c444-d85d6d9014e7"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "closest = Rot3.ClosestTo([\n",
        "    [1, 0, 0],\n",
        "    [0, 2, 0],\n",
        "    [0, 0, 3]\n",
        "])\n",
        "print(closest)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Sm3oUTObqxJl"
      },
      "source": [
        "## Properties"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "0Gax04WXqyeE"
      },
      "source": [
        "The following properties are available from the standard interface:\n",
        "- `matrix()`: Returns the 3x3 rotation matrix.\n",
        "- `transpose()`: Returns the transpose of the 3x3 rotation matrix.\n",
        "- `xyz()`: Returns the 3 Euler angles as $x, y, z$.\n",
        "- `ypr()`: Returns the 3 Euler angles as yaw, pitch, and roll.\n",
        "- `rpy()`: Returns the 3 Euler angles as roll, pitch, and yaw.\n",
        "- `roll()`: Returns the roll angle.\n",
        "- `pitch()`: Returns the pitch angle.\n",
        "- `yaw()`: Returns the yaw angle.\n",
        "- `axisAngle()`: Returns the axis-angle representation.\n",
        "- `toQuaternion()`: Returns the quaternion representation. The quaternion's attributes can then be accessed either individually with `w()`, `x()`, `y()`, `z()` or together with `coeffs()`.\n",
        "\n",
        "Note that accessing `roll()`, `pitch()`, and `yaw()` separately is less efficient than calling `rpy()` or `ypr()`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "zbNPBHiwDAE2",
        "outputId": "716f9db1-f7c7-4418-f7c7-5c6bae0b6a2c"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Matrix:\n",
            " [[ 5.30287619e-17 -1.00000000e+00  3.06161700e-17]\n",
            " [ 8.66025404e-01  6.12323400e-17  5.00000000e-01]\n",
            " [-5.00000000e-01  0.00000000e+00  8.66025404e-01]]\n",
            "Transpose:\n",
            " [[ 5.30287619e-17  8.66025404e-01 -5.00000000e-01]\n",
            " [-1.00000000e+00  6.12323400e-17  0.00000000e+00]\n",
            " [ 3.06161700e-17  5.00000000e-01  8.66025404e-01]]\n",
            "\n",
            "x, y, z: [0.         0.52359878 1.57079633]\n",
            "y, p, r: [1.57079633 0.52359878 0.        ]\n",
            "r, p, y: [0.         0.52359878 1.57079633]\n",
            "\n",
            "Roll:    0.0\n",
            "Pitch:   0.5235987755982988\n",
            "Yaw:     1.5707963267948966\n",
            "\n",
            "Axis-angle:\n",
            " (:-0.250563\n",
            " 0.250563\n",
            " 0.935113\n",
            ", 1.6378338249998232)\n",
            "\n",
            "Quaternion: [-0.1830127  0.1830127  0.6830127  0.6830127]\n"
          ]
        }
      ],
      "source": [
        "props = Rot3.RzRyRx(0, np.pi / 6, np.pi / 2)\n",
        "\n",
        "print(\"Matrix:\\n\", props.matrix())\n",
        "print(\"Transpose:\\n\", props.transpose())\n",
        "print()\n",
        "print(\"x, y, z:\", props.xyz())\n",
        "print(\"y, p, r:\", props.ypr())\n",
        "print(\"r, p, y:\", props.rpy())\n",
        "print()\n",
        "print(\"Roll:   \", props.roll())\n",
        "print(\"Pitch:  \", props.pitch())\n",
        "print(\"Yaw:    \", props.yaw())\n",
        "print()\n",
        "print(\"Axis-angle:\\n\", props.axisAngle())\n",
        "print()\n",
        "print(\"Quaternion:\", props.toQuaternion().coeffs())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "-XnTJ-psGeJf"
      },
      "source": [
        "## Basic operations"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "gj3OlBlGGfjj"
      },
      "source": [
        "`Rot3` can rotate and unrotate a 3D point or vector. Rotation is calculated by the simple matrix product $Rx$, and unrotation by $R^{-1}x$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "rtbkHyp3GgWx",
        "outputId": "43b1178c-39d3-4df2-face-9e8cef162cdd"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "[1.2246468e-16 2.0000000e+00 0.0000000e+00]\n",
            "[2. 0. 0.]\n",
            "[ 1.2246468e-16 -2.0000000e+00  0.0000000e+00]\n"
          ]
        }
      ],
      "source": [
        "z90 = Rot3.Rz(np.pi / 2)\n",
        "point = [2, 0, 0]\n",
        "\n",
        "# Rotate by 90 degrees around the Z axis\n",
        "rotated = z90.rotate(point)\n",
        "print(rotated)\n",
        "# Undo the rotation\n",
        "print(z90.unrotate(rotated))\n",
        "# Rotate backwards by 90 degrees around the Z axis\n",
        "print(z90.unrotate(point))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "d0bQ-tmwHmZ5"
      },
      "source": [
        "Check whether two `Rot3` instances are equal within a certain tolerance using `equals()`. Be careful with the `==` operator; it does not compare rotational equivalence, it compares object reference. If you wish to use more fine-grained equality comparison, convert to `np.ndarray` with `matrix()`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "MYiKxq4vItz3",
        "outputId": "e8f8fdd0-c539-476f-f233-2f78f98ca671"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "xyz.equals(ypr, 1e-8): True\n",
            "xyz == ypr: False\n",
            "xyz == xyz: True\n",
            "xyz.matrix() == ypr.matrix(): True\n"
          ]
        }
      ],
      "source": [
        "xyz = Rot3.RzRyRx(np.pi / 2, np.pi / 4, np.pi / 6)\n",
        "ypr = Rot3.Ypr(np.pi / 6, np.pi / 4, np.pi / 2)\n",
        "\n",
        "print(\"xyz.equals(ypr, 1e-8):\", xyz.equals(ypr, 1e-8))\n",
        "print(\"xyz == ypr:\", xyz == ypr)\n",
        "print(\"xyz == xyz:\", xyz == xyz)\n",
        "print(\"xyz.matrix() == ypr.matrix():\", np.all(xyz.matrix() == ypr.matrix()))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "bQzlRJ_2HWNz"
      },
      "source": [
        "Use SLERP (spherical linear interpolation) to interpolate between two `Rot3` instances. In terms of the Lie algebra (see below), SLERP can be calculated by scaling the log mapped relative rotation by the interpolation term $t$, then converting back to $\\text{SO}(3)$ using the exponential map. The formula is thus:\n",
        "\n",
        "$$\n",
        "R(t) = R_1 \\exp(t \\cdot \\log(R_1^{-1}R_2))\n",
        "$$\n",
        "\n",
        "where $R_1$ and $R_2$ are the start `Rot3` and end `Rot3` of the interpolation and $t$ is the interpolation term, usually but not necessarily in the range $[0, 1]$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "y45qZPivHkRR",
        "outputId": "e2320424-004f-47bf-e844-bf04df63a916"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t0.922613, 0.0523387, 0.382159;\n",
            "\t0.0523387, 0.964602, -0.258464;\n",
            "\t-0.382159, 0.258464, 0.887215\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "a = Rot3.RzRyRx(0, np.pi / 4, 0)\n",
        "b = Rot3.RzRyRx(np.pi / 6, 0, 0)\n",
        "\n",
        "print(a.slerp(0.5, b))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "XlmNuuxSGgoj"
      },
      "source": [
        "## Lie group $\\text{SO}(3)$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "XtUmE-QSGsh9"
      },
      "source": [
        "### Group operations\n",
        "\n",
        "`Rot3` implements the group operations `inverse`, `compose`, `between` and `identity`. For more information on groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "axvFPtxYdGru",
        "outputId": "977f9582-5b23-43c7-a6f5-a7bfce6d5cea"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "a:\n",
            " [[ 0.70710678 -0.70710678  0.        ]\n",
            " [ 0.70710678  0.70710678  0.        ]\n",
            " [ 0.          0.          1.        ]] \n",
            "b:\n",
            " [[ 1.000000e+00  0.000000e+00  0.000000e+00]\n",
            " [ 0.000000e+00  6.123234e-17 -1.000000e+00]\n",
            " [ 0.000000e+00  1.000000e+00  6.123234e-17]]\n"
          ]
        }
      ],
      "source": [
        "a = Rot3.Rz(np.pi / 4)\n",
        "b = Rot3.Roll(np.pi / 2)\n",
        "\n",
        "print(\"a:\\n\", a.matrix(), \"\\nb:\\n\", b.matrix())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "3eH9K5VH9jTb"
      },
      "source": [
        "The inverse of an $\\text{SO}(3)$ rotation matrix is the same as its transpose."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "ffVBzuOhGugd",
        "outputId": "ff207bed-850c-422a-d9a6-a0b59e801989"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t0.707107, 0.707107, 0;\n",
            "\t-0.707107, 0.707107, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "True\n"
          ]
        }
      ],
      "source": [
        "print(a.inverse())\n",
        "# The inverse is the same as the transpose.\n",
        "print(a.inverse().equals(Rot3(a.transpose()), 1e-8))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "P1bYYGkGdjxm"
      },
      "source": [
        "The product of the composition operation $A * B$ is the rotation matrix which applies the rotation of $A$ and then the rotation of $B$. The composition of two rotation matrices is just the product of the two matrices."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "4zXJJ77FdLBB",
        "outputId": "c5875121-0d97-475c-8669-93b92ac37c1b"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t0.707107, -4.32978e-17, 0.707107;\n",
            "\t0.707107, 4.32978e-17, -0.707107;\n",
            "\t0, 1, 6.12323e-17\n",
            "]\n",
            "\n",
            "True\n",
            "True\n"
          ]
        }
      ],
      "source": [
        "print(a.compose(b))\n",
        "\n",
        "# The * operator is syntactic sugar for the compose operation.\n",
        "print(a.compose(b).equals(a * b, 1e-8))\n",
        "\n",
        "# The composition of two rotation matrices is just the product of the matrices.\n",
        "print(np.all(a.compose(b).matrix() == a.matrix() @ b.matrix()))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "TIuwUygjfECu"
      },
      "source": [
        "The between operation calculates the rotation from one `Rot3` to another. It is defined as simply:\n",
        "\n",
        "$$\n",
        "R_{relative} = R_1^{-1}R_2\n",
        "$$"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "_qGyDV15dgmU",
        "outputId": "be748925-3425-41c7-b06b-57ab87955699"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t0.707107, 4.32978e-17, -0.707107;\n",
            "\t-0.707107, 4.32978e-17, -0.707107;\n",
            "\t0, 1, 6.12323e-17\n",
            "]\n",
            "\n",
            "True\n"
          ]
        }
      ],
      "source": [
        "print(a.between(b))\n",
        "print(a.between(b).equals(a.inverse() * b, 1e-8))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "6_jR6zhMfa1l"
      },
      "source": [
        "The identity is $I_3$, as described above."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "SchtjDIPfXtb",
        "outputId": "7d199a1e-b9a7-4775-81e7-9c1328012b89"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "print(Rot3.Identity())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fjfUIqKHflXY"
      },
      "source": [
        "#### Group operation invariants\n",
        "\n",
        "See that the following group invariants hold:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "ysQSPxuwfnen",
        "outputId": "4a7d8404-fc2a-46ca-ba18-236fd417382b"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Compose(a, Inverse(a)) == Identity:  True\n",
            "Compose(a, Between(a, b)) == b: True\n",
            "Between(a, b) == Compose(Inverse(a), b): True\n"
          ]
        }
      ],
      "source": [
        "print(\"Compose(a, Inverse(a)) == Identity: \", (a * a.inverse()).equals(Rot3.Identity(), 1e-8))\n",
        "print(\"Compose(a, Between(a, b)) == b:\", (a * a.between(b)).equals(b, 1e-8))\n",
        "print(\"Between(a, b) == Compose(Inverse(a), b):\", a.between(b).equals(a.inverse() * b, 1e-8))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "nKxTJy8YGuxg"
      },
      "source": [
        "### Lie group operations\n",
        "\n",
        "`Rot3` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html), and some detailed math in the [`SO3`](SO3.ipynb) class."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "MmBfK0ad1KZ6"
      },
      "source": [
        "The exponential map for $\\text{SO}(3)$ converts a 3D rotation vector (corresponding to a Lie algebra element in $\\mathfrak{so}(3)$) into a rotation matrix (Lie group element in $\\text{SO}(3)$). I.e., we map a rotation vector $\\boldsymbol{\\omega} \\in \\mathbb{R}^3$ to a rotation matrix $R \\in \\text{SO}(3)$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "yA5wO-5jGw2u",
        "outputId": "e0c75e07-2b6d-4f84-a90d-f1cceb3ad9fa"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 2.22045e-16, -1;\n",
            "\t0, 1, 2.22045e-16\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t6.12323e-17, 0.5, 0.866025;\n",
            "\t0, 0.866025, -0.5;\n",
            "\t-1, 3.06162e-17, 5.30288e-17\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t6.12323e-17, 0.866025, -0.5;\n",
            "\t0, -0.5, -0.866025;\n",
            "\t-1, 5.30288e-17, -3.06162e-17\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "r1 = Rot3.RzRyRx(np.pi / 6, np.pi / 2, 0)\n",
        "r2 = Rot3.RzRyRx(0, 0, np.pi / 4)\n",
        "p1 = [np.pi / 2, 0, 0]\n",
        "\n",
        "# The exponential map at identity creates a rotation using Rodrigues' formula.\n",
        "print(Rot3.Expmap(p1))\n",
        "# The retract function takes the exponential map of the supplied vector and\n",
        "# composes it with the calling Rot3. In other words, it maps from the tangent\n",
        "# space to the manifold.\n",
        "print(r1)\n",
        "print(r1.retract(p1))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Yk2nazsK6ixV"
      },
      "source": [
        "The logarithm map for $ \\text{SO}(3) $ is the inverse of the exponential map It converts a rotation matrix $ R \\in SO(3) $ into a 3D rotation vector (corresponding to a Lie algebra element in $ \\mathfrak{so}(3) $)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "0V2oQQ0lxS2-",
        "outputId": "62b40acb-799e-4a91-dacd-c9e0266665c3"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "[ 0.41038024  1.53155991 -0.41038024]\n",
            "[-1.01420581 -1.32173874  1.01420581]\n",
            "[-1.01420581 -1.32173874  1.01420581]\n"
          ]
        }
      ],
      "source": [
        "# Calculate the log map of r at identity. Returns the coordinates of the rotation\n",
        "# in the tangent space.\n",
        "print(Rot3.Logmap(r1))\n",
        "\n",
        "# Transform r2 into its vector representation relative to r1.\n",
        "print(r1.logmap(r2))\n",
        "# logmap is the same as calculating the coordinate of the second Rot3 in the\n",
        "# local frame of the first, which localCoordinates (inherited from LieGroup) does.\n",
        "print(r1.localCoordinates(r2))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "-kTgSGJS06EC",
        "outputId": "97051c49-284e-4ee8-d806-53f0d842fc31"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "R: [\n",
            "\t0.707107, -0.707107, 0;\n",
            "\t0.707107, 0.707107, 0;\n",
            "\t-0, 0, 1\n",
            "]\n",
            "\n",
            "R: [\n",
            "\t0.707107, -0.707107, 9.04269e-17;\n",
            "\t0.707107, 0.707107, 4.0637e-17;\n",
            "\t-6.77245e-17, 6.77245e-17, 1\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Applying localCoordinates and then retract cancels out, returning r2 given any\n",
        "# r1. This is because it transforms r2 from the manifold to the tangent space\n",
        "# using the log map, then transforms that result back into the manifold using\n",
        "# the exponential map.\n",
        "print(r2)\n",
        "print(r1.retract(r1.localCoordinates(r2)))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "s_qu2bGt1-vr"
      },
      "source": [
        "## Serialization\n",
        "\n",
        "A `Rot3` can be serialized to a string for saving, then later used by deserializing the string."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "m6ku7L_768Ta",
        "outputId": "8cb6fe04-6759-4cd9-8145-42f4fc2a72dd"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Before serialization: R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 6.12323e-17, -1;\n",
            "\t0, 1, 6.12323e-17\n",
            "]\n",
            "\n",
            "22 serialization::archive 15 1 0\n",
            "0 1.00000000000000000e+00 0.00000000000000000e+00 0.00000000000000000e+00 0.00000000000000000e+00 6.12323399573676604e-17 -1.00000000000000000e+00 0.00000000000000000e+00 1.00000000000000000e+00 6.12323399573676604e-17\n",
            "\n",
            "The serialized value is a string: <class 'str'>\n",
            "After deserialization: R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 6.12323e-17, -1;\n",
            "\t0, 1, 6.12323e-17\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "a = Rot3.Rx(np.pi / 2)\n",
        "print(\"Before serialization:\", a)\n",
        "\n",
        "str_val = a.serialize()\n",
        "print(str_val)\n",
        "print(\"The serialized value is a string:\", type(str_val))\n",
        "# Save to file, etc...\n",
        "\n",
        "b = Rot3()\n",
        "b.deserialize(str_val)\n",
        "print(\"After deserialization:\", b)"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": []
    },
    "kernelspec": {
      "display_name": "Python 3",
      "name": "python3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
